#include "lsm6ds3_selftest.h"



// self test

int32_t OUTX_NOST = 0, OUTY_NOST = 0, OUTZ_NOST = 0;
int32_t OUTX_ST = 0, OUTY_ST = 0, OUTZ_ST = 0;

/***************************************************ACC selftest**********************************************************************************/
static uint8_t sensor_acc_st_step1(void)
{
  uint8_t i, var[] = {0x30, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x00};
	uint8_t base = 0x10;
	
	for (i = 0; i < sizeof (var); i++) {
		if(LSM6DS3_IO_Write(&var[i], LSM6DS3_XG_MEMS_ADDRESS, base+i, 1) != IMU_6AXES_OK) {
			printf("ERR occurd on round %d\n", i);
			return 1;
		}
	}
	return 0;
}

static uint8_t sensor_acc_st_read_xyz(int16_t *x, int16_t *y, int16_t *z)
{
	uint8_t var = 0;
	uint8_t buf[2];
	
	while (1) {
	  if (LSM6DS3_IO_Read(&var, LSM6DS3_XG_MEMS_ADDRESS, 0x1E, 1) != IMU_6AXES_OK) {
			printf("ERR occurd on read %d\n", __LINE__);
			return 1;
		}
		// check ready bit
		if (var & 0x01) {
	    if (LSM6DS3_IO_Read(buf, LSM6DS3_XG_MEMS_ADDRESS, 0x28, 2) != IMU_6AXES_OK) {
			  printf("ERR occurd on read %d\n", __LINE__);
			  return 1;
		  }
			*x = ((((int16_t)buf[1]) << 8) + (int16_t)buf[0]);
			
	    if (LSM6DS3_IO_Read(buf, LSM6DS3_XG_MEMS_ADDRESS, 0x2A, 2) != IMU_6AXES_OK) {
			  printf("ERR occurd on read %d\n", __LINE__);
			  return 1;
		  }
			*y = ((((int16_t)buf[1]) << 8) + (int16_t)buf[0]);
			
	    if (LSM6DS3_IO_Read(buf, LSM6DS3_XG_MEMS_ADDRESS, 0x2C, 2) != IMU_6AXES_OK) {
			  printf("ERR occurd on read %d\n", __LINE__);
			  return 1;
		  }
			*z = ((((int16_t)buf[1]) << 8) + (int16_t)buf[0]);
			
			printf("x=%d, y=%d, z=%d\n", *x, *y, *z);
			return 0;
		}
	}
}

static uint8_t sensor_acc_st_read_xyz_5times_avg(int32_t *x, int32_t *y, int32_t *z)
{
	uint8_t loop = 0;
	int16_t tmpX, tmpY, tmpZ;
	
	while (loop++ < 5) {
		if (0 != sensor_acc_st_read_xyz(&tmpX, &tmpY, &tmpZ))
			return 1;
		
		*x += tmpX;
		*y += tmpY;
		*z += tmpZ;
	}
	
	// average
	*x /= loop;
	*y /= loop;
	*z /= loop;

	return 0;
}

// read out data and drop it.
static uint8_t sensor_acc_st_step2(void)
{
	int16_t x, y, z;
	
	if (0 != sensor_acc_st_read_xyz(&x, &y, &z))
		return 1;
	return 0;
}

// read out data * 5 times and calculate OUTX_NOST / OUTY_NOST / OUTZ_NOST.
static uint8_t sensor_acc_st_step3(void)
{
	sensor_acc_st_read_xyz_5times_avg(&OUTX_NOST, &OUTY_NOST, &OUTZ_NOST);
	
	printf("OUTX_NOST: %d\n", OUTX_NOST);
	printf("OUTY_NOST: %d\n", OUTY_NOST);
	printf("OUTZ_NOST: %d\n", OUTZ_NOST);
	
	return 0;
}

// enable SELF TEST.
static uint8_t sensor_acc_st_step4(void)
{
	uint8_t var;
	
	var = 0x01;
	if(LSM6DS3_IO_Write(&var, LSM6DS3_XG_MEMS_ADDRESS, 0x14, 1) != IMU_6AXES_OK) {
		printf("ERR occurd at %d\n", __LINE__);
		return 1;
	}
	return 0;
}

// read out data and drop it.
static uint8_t sensor_acc_st_step5(void)
{
	int16_t x, y, z;
	
	if (0 != sensor_acc_st_read_xyz(&x, &y, &z))
		return 1;
	return 0;
}

// read out data * 5 times and calculate OUTX_ST / OUTY_ST / OUTZ_ST.
static uint8_t sensor_acc_st_step6(void)
{
	uint8_t loop = 0;
	int16_t x, y, z;
	
	while (loop++ < 5) {
		if (0 != sensor_acc_st_read_xyz(&x, &y, &z))
			return 1;
		
		OUTX_ST += x;
		OUTY_ST += y;
		OUTZ_ST += z;
	}
	
	// average
	OUTX_ST /= loop;
	OUTY_ST /= loop;
	OUTZ_ST /= loop;
	
	printf("OUTX_ST: %d\n", OUTX_ST);
	printf("OUTY_ST: %d\n", OUTY_ST);
	printf("OUTZ_ST: %d\n", OUTZ_ST);
	
	return 0;
}

/*
For Accelerometer, Min= 90mg, Max= 1700mg;

For Gyro@2000dps,Min= 150dps, Max= 700dps;
*/
static uint8_t sensor_acc_st_step7(void)
{
	int32_t dX, dY, dZ;
	
	dX = (int32_t)((OUTX_ST - OUTX_NOST) * 2000 / 32768);
	dY = (int32_t)((OUTY_ST - OUTY_NOST) * 2000 / 32768);
	dZ = (int32_t)((OUTZ_ST - OUTZ_NOST) * 2000 / 32768);
	
	printf("dX=%d\n", dX);
	printf("dY=%d\n", dY);
	printf("dZ=%d\n", dZ);
	
	if (dX < 90 || dX > 1700)
		return 1;
	if (dY < 90 || dY > 1700)
		return 1;
	if (dZ < 90 || dZ > 1700)
		return 1;
	return 0;
}
	
uint8_t sensor_acc_selftest(void)
{
	if (0 != sensor_acc_st_step1())
		return 1;
	
	HAL_Delay(500);
	if (0 != sensor_acc_st_step2())
		return 1;
	
	if (0 != sensor_acc_st_step3())
		return 1;
	
	if (0 != sensor_acc_st_step4())
		return 1;
	
	if (0 != sensor_acc_st_step5())
		return 1;
	
	if (0 != sensor_acc_st_step6())
		return 1;
	
	if (0 != sensor_acc_st_step7())
		return 1;
	
 return 0;
}

/***************************************************GRO selftest**********************************************************************************/

static uint8_t sensor_gro_st_step1(void)
{
  uint8_t i, var[] = {0x00, 0x5c, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x38};
	uint8_t base = 0x10;
	
	for (i = 0; i < sizeof (var); i++) {
		if(LSM6DS3_IO_Write(&var[i], LSM6DS3_XG_MEMS_ADDRESS, base+i, 1) != IMU_6AXES_OK) {
			printf("ERR occurd on round %d\n", i);
			return 1;
		}
	}
	return 0;
}

static uint8_t sensor_gro_st_read_xyz(int16_t *x, int16_t *y, int16_t *z)
{
	uint8_t var = 0;
	uint8_t buf[2];
	
	while (1) {
	  if (LSM6DS3_IO_Read(&var, LSM6DS3_XG_MEMS_ADDRESS, 0x1E, 1) != IMU_6AXES_OK) {
			printf("ERR occurd on read %d\n", __LINE__);
			return 1;
		}
		// check ready bit
		if (var & 0x02) {
	    if (LSM6DS3_IO_Read(buf, LSM6DS3_XG_MEMS_ADDRESS, 0x22, 2) != IMU_6AXES_OK) {
			  printf("ERR occurd on read %d\n", __LINE__);
			  return 1;
		  }
			*x = ((((int16_t)buf[1]) << 8) + (int16_t)buf[0]);
			
	    if (LSM6DS3_IO_Read(buf, LSM6DS3_XG_MEMS_ADDRESS, 0x24, 2) != IMU_6AXES_OK) {
			  printf("ERR occurd on read %d\n", __LINE__);
			  return 1;
		  }
			*y = ((((int16_t)buf[1]) << 8) + (int16_t)buf[0]);
			
	    if (LSM6DS3_IO_Read(buf, LSM6DS3_XG_MEMS_ADDRESS, 0x26, 2) != IMU_6AXES_OK) {
			  printf("ERR occurd on read %d\n", __LINE__);
			  return 1;
		  }
			*z = ((((int16_t)buf[1]) << 8) + (int16_t)buf[0]);
			
			printf("x=%d, y=%d, z=%d\n", *x, *y, *z);
			return 0;
		}
	}
}

static uint8_t sensor_gro_st_read_xyz_5times_avg(int32_t *x, int32_t *y, int32_t *z)
{
	uint8_t loop = 0;
	int16_t tmpX, tmpY, tmpZ;
	
	while (loop++ < 5) {
		if (0 != sensor_gro_st_read_xyz(&tmpX, &tmpY, &tmpZ))
			return 1;
		
		*x += tmpX;
		*y += tmpY;
		*z += tmpZ;
	}
	
	// average
	*x /= loop;
	*y /= loop;
	*z /= loop;

	return 0;
}

// read out data and drop it.
static uint8_t sensor_gro_st_step2(void)
{
	int16_t x, y, z;
	
	if (0 != sensor_gro_st_read_xyz(&x, &y, &z))
		return 1;
	return 0;
}

// read out data * 5 times and calculate OUTX_NOST / OUTY_NOST / OUTZ_NOST.
static uint8_t sensor_gro_st_step3(void)
{
	sensor_gro_st_read_xyz_5times_avg(&OUTX_NOST, &OUTY_NOST, &OUTZ_NOST);
	
	printf("OUTX_NOST: %d\n", OUTX_NOST);
	printf("OUTY_NOST: %d\n", OUTY_NOST);
	printf("OUTZ_NOST: %d\n", OUTZ_NOST);
	
	return 0;
}

// enable SELF TEST.
static uint8_t sensor_gro_st_step4(void)
{
	uint8_t var;
	
	var = 0x04;
	if(LSM6DS3_IO_Write(&var, LSM6DS3_XG_MEMS_ADDRESS, 0x14, 1) != IMU_6AXES_OK) {
		printf("ERR occurd at %d\n", __LINE__);
		return 1;
	}
	return 0;
}

// read out data and drop it.
static uint8_t sensor_gro_st_step5(void)
{
	int16_t x, y, z;
	
	if (0 != sensor_gro_st_read_xyz(&x, &y, &z))
		return 1;
	return 0;
}

// read out data * 5 times and calculate OUTX_ST / OUTY_ST / OUTZ_ST.
static uint8_t sensor_gro_st_step6(void)
{
	uint8_t loop = 0;
	int16_t x, y, z;
	
	while (loop++ < 5) {
		if (0 != sensor_gro_st_read_xyz(&x, &y, &z))
			return 1;
		
		OUTX_ST += x;
		OUTY_ST += y;
		OUTZ_ST += z;
	}
	
	// average
	OUTX_ST /= loop;
	OUTY_ST /= loop;
	OUTZ_ST /= loop;
	
	printf("OUTX_ST: %d\n", OUTX_ST);
	printf("OUTY_ST: %d\n", OUTY_ST);
	printf("OUTZ_ST: %d\n", OUTZ_ST);
	
	return 0;
}

/*
For Accelerometer, Min= 90mg, Max= 1700mg;

For Gyro@2000dps,Min= 150dps, Max= 700dps;
*/
static uint8_t sensor_gro_st_step7(void)
{
	int32_t dX, dY, dZ;
	
	dX = (int32_t)((OUTX_ST - OUTX_NOST) * 2000 / 32768);
	dY = (int32_t)((OUTY_ST - OUTY_NOST) * 2000 / 32768);
	dZ = (int32_t)((OUTZ_ST - OUTZ_NOST) * 2000 / 32768);
	
	printf("dX=%d\n", dX);
	printf("dY=%d\n", dY);
	printf("dZ=%d\n", dZ);
	
	if (dX < 150 || dX > 700)
		return 1;
	if (dY < 150 || dY > 700)
		return 1;
	if (dZ < 150 || dZ > 700)
		return 1;
	return 0;
}
	
	
uint8_t sensor_gyro_selftest(void)
{

	
	if (0 != sensor_gro_st_step1())
		return 1;
	
	HAL_Delay(500);
	if (0 != sensor_gro_st_step2())
		return 1;
	
	if (0 != sensor_gro_st_step3())
		return 1;
	
	if (0 != sensor_gro_st_step4())
		return 1;
	
	if (0 != sensor_gro_st_step5())
		return 1;
	
	if (0 != sensor_gro_st_step6())
		return 1;
	
	if (0 != sensor_gro_st_step7())
		return 1;
		
	
 return 0;
}


